import sensor, image, time,pyb,math
from pyb import UART,LED
from time import sleep_ms

sensor.reset()
sensor.set_pixformat(sensor.RGB565) # 灰度更快(160x120 max on OpenMV-M7)
sensor.set_framesize(sensor.QQVGA)
sensor.skip_frames(time = 2000)
sensor.skip_frames(10) # 让新的设置生效。
sensor.set_auto_whitebal(False) # turn this off.
#关闭白平衡。白平衡是默认开启的，在颜色识别中，需要关闭白平衡。
clock = time.clock()

uart = UART(3, 115200)

green_threshold=[(30, 100, 15, 127, 15, 127),
                (0, 100, 30, 127, -128, 127),#
                (63, 72, 22, 55, -1, 15),
                (61, 71, 29, 69, -7, 11)]
roi2 = [(24,9,228,214)]
def find_max(blobs):    #定义寻找色块面积最大的函数
    max_size=0
    for blob in blobs:
        if blob.pixels() > max_size:
            max_blob=blob
            max_size = blob.pixels()
    return max_blob
flag=0
recflag = 0
while(True):
    clock.tick()
    img = sensor.snapshot()
    for r in img.find_rects(threshold = 1000):
        if r.magnitude()>40000:
            img.draw_rectangle(r.rect(), color = (255, 0, 0))
            for p in r.corners(): img.draw_circle(p[0], p[1], 5, color = (0, 255, 0))
            uart.write(bytearray([0x2C]))
            for i in r.corners():
                output_str = "%c" % i[0]
                uart.write(output_str)
                output_str = "%c" % i[1]
                uart.write(output_str)
                flag+=1
                print(i[0],i[1])
            uart.write(bytearray([0x5B]))
            print("\r\n")
    if flag==16:
        sensor.set_framesize(sensor.QVGA)
        break
while(True):
    clock.tick()
    img = sensor.snapshot()

    blobs = img.find_blobs(green_threshold,roi=roi2[0])
    if blobs:
    #如果找到了目标颜色
        b = find_max(blobs)
        img.draw_rectangle(b[0:4]) # rect
        #用矩形标记出目标颜色区域
        img.draw_cross(b[5], b[6]) # cx, cy
        #在目标颜色区域的中心画十字形标记
        uart.write('%')
        output_str = "%c" % b[5]        #中心点X坐标
        uart.write(output_str)
        output_str = "%c" % b[6]        #中心点Y坐标
        uart.write(output_str)
        uart.write('!')
        print(b[5], b[6])
        sleep_ms(10)
